import math
import queue

class LinearPlanner:
    def generate(self, start, end, speed, dt=0.05):  # 调整 dt 值越大，发送的点跨度更大，机械臂运动越快
        distance = math.sqrt(sum((e - s)**2 for s, e in zip(start, end)))
        if distance < 1e-6:  # 忽略微小距离
            return [end.copy()]  # 直接返回终点
        
        total_time = distance / speed
        points = []
        current_time = 0.0
        
        while current_time <= total_time + 1e-9:
            ratio = min(current_time / total_time, 1.0)
            point = [s + (e - s) * ratio for s, e in zip(start, end)]
            points.append(point)
            current_time += dt
        
        # 确保最后一个点是精确的终点
        points[-1] = [float(e) for e in end]
        self.total_steps = len(points)
        return points

class AxisAnglePlanner:
    def generate(self, start_angles, end_angles, max_speed, dt=0.05):
        """
        生成关节空间轴角运动路径
        :param start_angles: 起始关节角度列表（单位：度）
        :param end_angles: 目标关节角度列表（单位：度）
        :param max_speed: 最大关节角速度（度/秒）
        :param dt: 时间间隔（秒）
        :return: 包含路径点的Queue对象
        """
        # 计算各关节角度差
        deltas = [end - start for start, end in zip(start_angles, end_angles)]
        
        # 找到需要最大运动时间的关节
        max_duration = 0
        for delta in deltas:
            duration = abs(delta) / max_speed
            if duration > max_duration:
                max_duration = duration

        if max_duration == 0:
            return queue.Queue()

        q = queue.Queue()
        current_time = 0.0
        
        # 生成运动路径（包含浮点精度容差）
        while current_time <= max_duration + 1e-9:
            # 计算各关节运动进度
            progress = min(current_time / max_duration, 1.0)
            
            # 计算各关节当前角度
            angles = [
                start + delta * progress
                for start, delta in zip(start_angles, deltas)
            ]
            q.put(angles)
            
            current_time += dt

        self.total_steps = q.qsize()
        return q

class CircularPlanner:
    ## 可扩展圆弧插补
    pass